#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import rospy
import actionlib

from std_msgs.msg import Float64
from geometry_msgs.msg import Twist

from npu_robot.msg import RobotTrackAction, RobotTrackFeedback, RobotTrackResult

# 设置相机分辨率
CAMERA_WIDTH = 640
CAMERA_HEIGHT = 480

# 偏差阈值
BIAS_THRESHOLD = 0.15
# 控制信号比例
CONTROL_RATIO = 0.25


class RobotTrackActionServer():
    # 创建用于发布反馈(feedback)/结果(result)的消息
    _feedback = RobotTrackFeedback()
    _result = RobotTrackResult()

    def __init__(self):
        # 小车跟随垃圾服务端
        self._action_server = actionlib.SimpleActionServer('/robot_track', RobotTrackAction, execute_cb=self.execute_callback, auto_start=False)

        # 物体中心点横坐标订阅者
        self.object_center_x_sub = rospy.Subscriber('/camera/image_raw/object_center_x', Float64, self.object_center_x_callback, queue_size=1)

        # 物体中心点纵坐标订阅者
        self.object_center_y_sub = rospy.Subscriber('/camera/image_raw/object_center_y', Float64, self.object_center_y_callback, queue_size=1)

        # 小车速度发布者
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

        self.linear_speed = 0.0
        self.rotation_speed = 0.0

        self.twist = Twist()
        self.twist.linear.x = self.linear_speed
        self.twist.linear.y = 0
        self.twist.linear.z = 0
        self.twist.angular.x = 0
        self.twist.angular.y = 0
        self.twist.angular.z = self.rotation_speed

        # 物体中心点回调次数
        self.object_center_x_callback_times = 0
        self.object_center_y_callback_times = 0
        # 物体中心点回调次数阈值
        self.object_center_x_callback_times_threshold = 3
        self.object_center_y_callback_times_threshold = 3

        # 小车到达偏差阈值之内的次数
        self.bias_x_correct_times = 0
        self.bias_y_correct_times = 0
        # 小车到达偏差阈值之内的次数阈值
        self.bias_x_correct_times_threshold = 50
        self.bias_y_correct_times_threshold = 50

        # 标志位 - action是否完成
        self.flag_x = False
        self.flag_y = False

        # 启动服务端
        self._action_server.start()

        rospy.spin()

    def execute_callback(self, goal):
        """
        action请求回调
        """
        rospy.logdebug('robot track action execute callback...')

        while not (self.flag_x and self.flag_y):
            self.twist.linear.x = self.linear_speed
            self.twist.angular.z = self.rotation_speed
            rospy.logdebug('publish twist..., linear_speed is %s', self.linear_speed)
            rospy.logdebug('rotation_speed is %s', self.rotation_speed)
            self.cmd_vel_pub.publish(self.twist)

        self.flag_x = False
        self.flag_y = False
        self.twist.linear.x = 0
        self.twist.angular.z = 0
        self.cmd_vel_pub.publish(self.twist)
        # 发布结果(result)
        rospy.loginfo('Robot Track Succeeded')
        self._result.result_state = 1
        self._action_server.set_succeeded(self._result)

    def object_center_x_callback(self, data):
        """
        物体中心点横坐标回调函数
        """
        rospy.logdebug('object center x callback...')

        # 确保行为还没有被取消
        if not self._action_server.is_active():
            return

        self.object_center_x_callback_times += 1
        if self.object_center_x_callback_times == self.object_center_x_callback_times_threshold:
            self.object_center_x_callback_times = 0

            object_center_x = data.data
            rospy.logdebug('object center x: %s', object_center_x)

            # 计算物体中心与图像中点偏差
            bias_x = round((object_center_x - CAMERA_WIDTH/2)/(CAMERA_WIDTH/2), 3)
            rospy.logdebug('bias_x is %s', bias_x)

            # 判断是否在偏差阈值之内
            if bias_x < BIAS_THRESHOLD and bias_x > (-BIAS_THRESHOLD):
                bias_x = 0

                self.bias_x_correct_times += 1
                if self.bias_x_correct_times == self.bias_x_correct_times_threshold:
                    self.bias_x_correct_times = 0
                    self.flag_x = True

            # 比例控制信号计算
            control_x = CONTROL_RATIO * bias_x
            rospy.logdebug('control x: %s', control_x)

            # 调整关节状态
            self.rotation_speed = -control_x

            # 发布反馈(feedback)
            self._feedback.feedback_state = 1
            self._action_server.publish_feedback(self._feedback)

        rospy.logdebug('object center x callback OK')

    def object_center_y_callback(self, data):
        """
        物体中心点横坐标回调函数
        """
        rospy.logdebug('object center y callback...')

        # 确保行为还没有被取消
        if not self._action_server.is_active():
            return

        self.object_center_y_callback_times += 1
        if self.object_center_y_callback_times == self.object_center_y_callback_times_threshold:
            self.object_center_y_callback_times = 0

            object_center_y = data.data
            rospy.logdebug('object center y: %s', object_center_y)

            # 计算物体中心与图像中点偏差
            bias_y = round((object_center_y - CAMERA_HEIGHT/2)/(CAMERA_HEIGHT/2), 3)

            # 判断是否在偏差阈值之内
            if bias_y < BIAS_THRESHOLD and bias_y > (-BIAS_THRESHOLD):
                bias_y = 0

                self.bias_y_correct_times += 1
                if self.bias_y_correct_times == self.bias_y_correct_times_threshold:
                    self.bias_y_correct_times = 0
                    self.flag_y = True

            # 比例控制信号计算
            control_y = CONTROL_RATIO * bias_y
            rospy.logdebug('control y: %s', control_y)

            # 调整关节状态
            self.linear_speed = -control_y

            # 发布反馈(feedback)
            self._feedback.feedback_state = 1
            self._action_server.publish_feedback(self._feedback)

        rospy.logdebug('object center y callback OK')


if __name__ == '__main__':
    rospy.init_node('npu_robot_track_server', anonymous=True, log_level=envs.log_level)
    robot_track_server = RobotTrackActionServer()
